/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "StdAfx.h"
#include "Frame2.h"

namespace RobotPlant { namespace Math{
	Frame2::Frame2(void)
	{
	}

	Frame2::~Frame2(void)
	{
	}

	Frame2::Frame2(const Frame2 & arg) : Translation(arg.Translation), Rotation(arg.Rotation)
	{
	}

	Frame2::Frame2(const Vector2 & translation)
	{
		Translation = translation;
		Rotation = Rotation2::Identity();
	}

	Frame2::Frame2(const Rotation2 & arg)
	{
		Translation = Vector2::Zero();
		Rotation = arg;
	}

	Frame2::Frame2(const Rotation2 & rotation, const Vector2 & translation)
	{
		Rotation = rotation;
		Translation = translation;
	}

	double Frame2::operator () (const int row,const int column) const
	{
		//validate input arguments
		if( (row < 0) || (row > 2) )
		{
			throw std::exception("Argument row must be between 0 and 2.");
		}
		if( (column < 0) || (column > 2) )
		{
			throw std::exception("Argument row must be between 0 and 2.");
		}

		if (row == 2) 
		{
			if (column == 2)
				return 1;
			else
				return 0;
		}
		else
		{
			if (column == 2) 
				return Translation[row];
			else
				return Rotation(row,column);
		}
	}

	Frame2& Frame2::operator =(const Frame2 & arg)
	{ 
		Rotation = arg.Rotation;
		Translation = arg.Translation;
		return *this;
	}

	bool operator == ( const Frame2& lhs, const Frame2& rhs )
	{
		return Equal(lhs,rhs);
	}

	bool operator != ( const Frame2& lhs, const Frame2& rhs )
	{
		return !Equal(lhs,rhs);
	}

	Vector2 operator *(const Frame2& lhs, const Vector2& rhs)
	{
		return lhs.Rotation * rhs + lhs.Translation;
	}

	Frame2 operator *(const Frame2& lhs, const Frame2& rhs)
	{
		return Frame2(lhs.Rotation * rhs.Rotation, lhs.Rotation * rhs.Translation + lhs.Translation);
	}

	void Frame2::SetInverse()
	{
		Rotation.SetInverse();
		Translation =  Rotation * Translation ;
		Translation.ReverseSign();
	}

	Frame2 Frame2::Inverse() const
	{
		Frame2 tmp(*this);
		tmp.SetInverse();
		return tmp;
	}

	Vector2 Frame2::Inverse(const Vector2& arg) const
	{
		return Rotation.Inverse(arg - Translation);
	}

	void Frame2::SetIdentity()
	{
		Rotation.SetIdentity();
		Translation = Vector2::Zero();
	}

	Frame2 Frame2::Identity()
	{
		Frame2 tmp;
		tmp.SetIdentity();
		return tmp;
	}

	bool Equal(const Frame2& a,const Frame2& b,const double eps)
	{
		return ( Equal(a.Translation, b.Translation, eps) && Equal(a.Rotation, b.Rotation, eps));
	}

	//Frame2 AddDelta(const Frame2& a,const Twist& da,double dt)
	//{
	//	return Frame2( AddDelta(a.Rotation, da.Rot, dt),
	//		AddDelta(a.Translation, da.Vel, dt));
	//}

	//Twist Diff(const Frame3& F_a_b1,const Frame3& F_a_b2,double dt)
	//{
	//	return Twist(
	//		Diff(F_a_b1.Translation, F_a_b2.Translation, dt),
	//		Diff(F_a_b1.Rotation, F_a_b2.Rotation, dt)
	//		);
	//}

	void Random(Frame2& f)
	{
		Random(f.Rotation);
		Random(f.Translation);
	}

	void PosRandom(Frame2& f)
	{
		Random(f.Rotation);
		Random(f.Translation);
	}
}}
